#include "omni_wheel.h"
#include "omni_wheel_cal.h"
#include "M3508.h"
#include "dbus.h"
#include "interface.h"
#include "math.h"
#include "bsp_exti.h"
Wheel_Data Small_cake={0};

void dbus_omni_wheel_speed()
{
	//dbus_ctrl_data.channel.CH[2]=1122;
	Small_cake.Robot_in_self.velocity_exp.Vx = (dbus_ctrl_data.channel.CH[2]-0x400)*4;
	Small_cake.Robot_in_self.velocity_exp.Vy = (dbus_ctrl_data.channel.CH[3]-0x400)*4;
	Small_cake.Robot_in_self.velocity_exp.Vw = (dbus_ctrl_data.channel.rolling_wheel-0x400)*10-(dbus_ctrl_data.channel.CH[0]-0x400)*K;
	Small_cake.Robot_in_self.position_exp.yaw = (dbus_ctrl_data.channel.CH[0]-0x400)*180/660;
	//Pos = ((float)dbus_ctrl_data.channel.CH[0]-364)*8191/1320;
} 

void omni_wheel_speed_management(Wheel_Data* Small_cake)
{
	float V_sum=0;
	V_sum=fabs(Small_cake->Robot_in_world.velocity_exp.Vx)+
	      fabs(Small_cake->Robot_in_world.velocity_exp.Vy)+
	      fabs(Small_cake->Robot_in_world.velocity_exp.Vw);
	
	if(V_sum>50)
	{
		Small_cake->Robot_in_world.velocity_exp.Vx/=V_sum;
		Small_cake->Robot_in_world.velocity_exp.Vy/=V_sum;
		Small_cake->Robot_in_world.velocity_exp.Vw/=V_sum;
		if(1)
		{
			Small_cake->power_factor=10;
		}
		else if(1)
		{
			Small_cake->power_factor=10;
		}
		else if(1)
		{
			Small_cake->power_factor=10;
		}
		else
	    {
		    Small_cake->power_factor= 0;
	    }
		
		if(Small_cake->power_factor > 450)
	    {
		    Small_cake->power_factor = 450;
	    }
	    else if(Small_cake->power_factor <= 0)
	    {
	    	Small_cake->power_factor = 0;
	    }
	}
	
	if(fabs(Small_cake->Robot_in_world.velocity_exp.Vx)+
	   fabs(Small_cake->Robot_in_world.velocity_exp.Vy)+
	   fabs(Small_cake->Robot_in_world.velocity_exp.Vw)<0.3)
	{
		Small_cake->Robot_in_world.velocity_exp.Vx=0;
		Small_cake->Robot_in_world.velocity_exp.Vy=0;
		Small_cake->Robot_in_world.velocity_exp.Vw=0;
		return;
	}
	else
	{
		Small_cake->Robot_in_world.velocity_exp.Vx*=Small_cake->power_factor;
		Small_cake->Robot_in_world.velocity_exp.Vy*=Small_cake->power_factor;
		Small_cake->Robot_in_world.velocity_exp.Vw*=Small_cake->power_factor;
	}
}

void omni_wheel_execute()
{
	//dbus_omni_wheel_speed();
	interface_change();
	Robot_Velocity_To_Wheel_Speed(&Small_cake);
	Wheel_Data_To_Motor(&Small_cake,&M3508_Motor);
	speed_ctrl(&M3508_Motor,M3508_Motor.M3508[0].exp_speed,M3508_Motor.M3508[1].exp_speed,M3508_Motor.M3508[2].exp_speed,M3508_Motor.M3508[3].exp_speed,DJI_M3508);
}
	